#include "priority.h"

#include "port/porting.h"
#include "ring_fifo/ring_fifo.h"
#include "comm/lead_zeros.h"

#if (PRIORITY_LEVEL_MAX == 8)
#define CntLeadZeros CntLeadZeros08
typedef uint8_t map_type;
#elif (PRIORITY_LEVEL_MAX == 16)
#define CntLeadZeros CntLeadZeros16
typedef uint16_t map_type;
#elif (PRIORITY_LEVEL_MAX == 32)
#define CntLeadZeros CntLeadZeros32
typedef uint32_t map_type;
#elif (PRIORITY_LEVEL_MAX == 64)
#define CntLeadZeros CntLeadZeros64
typedef uint64_t map_type;
#else
#error "the priority level can only be 8 or 16 or 32 or 64"
#endif

#define MAP_BIT_MASK (map_type)((map_type)0x1 << (PRIORITY_LEVEL_MAX - 1))

#define GNUC_MIN_VERSION_SUPPORT                                                   \
    ((__GNUC__ > 4) ||                                                             \
    ((__GNUC__ == 4) && (__GNUC_MINOR__ > 1)) ||                                   \
    ((__GNUC__ == 4) && (__GNUC_MINOR__ == 1) && (__GNUC_PATCHLEVEL__ >= 2)))

struct priority_queue_t {
    struct qmutex_t *mutex;
    struct qsem_t *sem;
    #if (GNUC_MIN_VERSION_SUPPORT)
    volatile map_type map;//用于快速定位当前优先级最高且有数据的fifo
    #endif/*GNUC_MIN_VERSION_SUPPORT*/
    struct ring_fifo_t *ring[PRIORITY_LEVEL_MAX];
};

static inline bool is_power_of_2(uint32_t n);

static inline bool is_power_of_2(uint32_t n)
{
	return ((n != 0) && ((n & (n - 1)) == 0));
}

uint32_t priority_queue_init(struct priority_queue_t **__priority_queue)
{
    struct priority_queue_t *_priority_queue = NULL;

    _priority_queue = (struct priority_queue_t *)malloc(sizeof(struct priority_queue_t));
    if(!_priority_queue)
    {
        return PQ_MALLOC_FAILED;
    }
    memset(_priority_queue, 0, sizeof(struct priority_queue_t));

    if(true != porting_init(&_priority_queue->mutex, &_priority_queue->sem))
    {
        return PQ_PORTING_INIT_ERROR;
    }

    #if (GNUC_MIN_VERSION_SUPPORT)
    _priority_queue->map = 0;
    #endif/*GNUC_MIN_VERSION_SUPPORT*/

    *__priority_queue = _priority_queue;

    return PQ_SUCCESS;
}

uint32_t priority_queue_add(struct priority_queue_t *_priority_queue, uint32_t prio, uint32_t size, enum pq_fifo_type type)
{
    if(prio >= PRIORITY_LEVEL_MAX) { return PQ_EXCEED_MAX_PRIORITY; }
    if(_priority_queue->ring[prio]) { return PQ_PRIORITY_ALREADY_EXISTS; }
    if(!is_power_of_2(size)) { return PQ_IS_NOT_POWER_OF_2; }

    if(RF_SUCCESS != ring_fifo_init(&_priority_queue->ring[prio], NULL, size, (enum ring_fifo_type)type))
    {
        return PQ_FIFO_INIT_ERROR;
    }

    return PQ_SUCCESS;
}

uint32_t priority_queue_in(struct priority_queue_t *_priority_queue, uint32_t prio, const uint8_t *pBuf, uint32_t len)
{
    if(true != _priority_queue->mutex->lock(_priority_queue->mutex))
    {
        return PQ_LOCK_ERROR;
    }

    if(RF_SUCCESS != ring_fifo_write(_priority_queue->ring[prio], pBuf, len))
    {
        _priority_queue->mutex->unlock(_priority_queue->mutex);

        return PQ_FIFO_WRITE_ERROR;
    }

    #if (GNUC_MIN_VERSION_SUPPORT)
    __sync_fetch_and_or(&_priority_queue->map, MAP_BIT_MASK >> prio);
    #endif/*GNUC_MIN_VERSION_SUPPORT*/
    
    _priority_queue->sem->post(_priority_queue->sem);

    _priority_queue->mutex->unlock(_priority_queue->mutex);

    return PQ_SUCCESS;
}

uint32_t priority_queue_out(struct priority_queue_t *_priority_queue, uint8_t *pBuf, uint32_t len, uint16_t *retLen)
{
    uint32_t z;

    do {
        #if (GNUC_MIN_VERSION_SUPPORT)
        //利用前导零快速查找有数据的高优先级fifo
        z = CntLeadZeros(_priority_queue->map);
        #else
        //遍历寻找有数据的高优先级fifo
        for(z = 0;z < PRIORITY_LEVEL_MAX;z++)
        {
            if(_priority_queue->ring[z] && (!ring_fifo_is_empty(_priority_queue->ring[z])))
            {
                break;
            }
        }
        #endif/*GNUC_MIN_VERSION_SUPPORT*/

        if(z >= PRIORITY_LEVEL_MAX)
		{
			if(true != _priority_queue->sem->wait(_priority_queue->sem))
			{
				return PQ_SEM_ERROR;
			}
			continue;
		}
        break;
    }while(true);
    if(RF_SUCCESS != ring_fifo_read(_priority_queue->ring[z], pBuf, len, retLen))
    {
        return PQ_FIFO_READ_ERROR;
    }
    #if (GNUC_MIN_VERSION_SUPPORT)
    if(ring_fifo_is_empty(_priority_queue->ring[z]))
    {
        __sync_fetch_and_and(&_priority_queue->map, ~(MAP_BIT_MASK >> z));
    }
    #endif/*GNUC_MIN_VERSION_SUPPORT*/

    return PQ_SUCCESS;
}

void priority_queue_destroy(struct priority_queue_t *_priority_queue)
{
    for(uint32_t i = 0;i < sizeof(_priority_queue->ring) / sizeof(*_priority_queue->ring);i++)
    {
        if(_priority_queue->ring[i])
        {
            ring_fifo_destroy(_priority_queue->ring[i]);
            _priority_queue->ring[i] = NULL;
        }
    }

    porting_destroy(_priority_queue->mutex, _priority_queue->sem);

    free(_priority_queue);
}

bool priority_queue_is_empty(struct priority_queue_t *_priority_queue)
{
    uint32_t i;

    for(i = 0;i < sizeof(_priority_queue->ring) / sizeof(*_priority_queue->ring);i++)
    {
        if(_priority_queue->ring[i])
        {
            if(!ring_fifo_is_empty(_priority_queue->ring[i]))
            {
                break;
            }
        }
    }

    if(i == sizeof(_priority_queue->ring) / sizeof(*_priority_queue->ring))
    {
        return true;
    }

    return false;
}
